// 数据池
#include "common/data_pool.h"
// ros
#include "rclcpp/rclcpp.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include <geometry_msgs/msg/point.hpp>
// proto
#include "msg_interfaces/msg/matrix_proto.hpp"
#include "perception_common.pb.h"
#include "perception_obstacle.pb.h"
namespace perception
{
    using std::placeholders::_1;
    using namespace std::chrono_literals;
    class PerceptionVisualization : public rclcpp::Node
    {
    public:
        PerceptionVisualization() : Node("perception_visualization")
        {
            lidar_objs_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("/lidar_perception/lidar_objs_vis", 10);
            lidar_objs_track_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("/lidar_perception/lidar_objs_track_vis", 10);
            lidar_objs_text_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("/lidar_perception/lidar_objs_vis_text", 10);
            timer_ = this->create_wall_timer(
                100ms, std::bind(&PerceptionVisualization::TimerCallback, this));
        }
        ~PerceptionVisualization(){};

    private:
        const int color[7][3] = {{187, 255, 255},
                                 {255, 193, 37},
                                 {69, 139, 116},
                                 {119, 136, 153},
                                 {0, 0, 205},
                                 {0, 255, 0},
                                 {255, 0, 0}};
        const double degree2arc = 3.1415926 / 180.0;
        const double arc2degree = 180 / 3.1415926;
        rclcpp::TimerBase::SharedPtr timer_;

        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr lidar_objs_pub;
        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr lidar_objs_track_pub;
        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr lidar_objs_text_pub;
        void TimerCallback()
        {
            DataPool *DP = DataPool::Instance();
            RCLCPP_WARN(this->get_logger(), "cluster size:%d", DP->main_data_.lidar_cells_data_.lidar_objs.size());
            if (!DP->main_data_.lidar_cells_data_.lidar_objs.empty())
            {
                visualization_msgs::msg::MarkerArray markers;
                visualization_msgs::msg::MarkerArray markers2;
                visualization_msgs::msg::MarkerArray markers3;
                int i = 0;
                for (auto obj : DP->main_data_.lidar_cells_data_.lidar_objs)
                {
                    // RCLCPP_INFO(this->get_logger(), "id:%d,type:%d,x:%f, y:%f,length:%f,width:%f,height:%f,heading:%f,age:%d,vx:%f,vy:%f",
                    //             obj.id, obj.type, obj.x, obj.y, obj.length, obj.width, obj.height, obj.heading, obj.age, obj.vx, obj.vy);
                    visualization_msgs::msg::Marker marker;
                    marker.header.frame_id = "base_link";
                    marker.header.stamp = rclcpp::Clock().now();
                    marker.lifetime.sec = 0;
                    marker.lifetime.nanosec = 1 * 1e8;
                    marker.type = visualization_msgs::msg::Marker::CUBE;
                    marker.id = obj.id;
                    // RCLCPP_INFO(this->get_logger(), "id:%d", obj.id);
                    marker.scale.x = obj.length;
                    marker.scale.y = obj.width;
                    marker.scale.z = obj.height;
                    int color_num = obj.type % 7;
                    // marker.action = obj.age;

                    marker.color.a = 0.5;
                    marker.color.r = color[color_num][0];
                    marker.color.g = color[color_num][1];
                    marker.color.b = color[color_num][2];

                    marker.pose.position.x = obj.x;
                    marker.pose.position.y = obj.y;
                    marker.pose.position.z = obj.z;
                    // std::cout <<
                    marker.pose.orientation.w = cos(obj.heading / 2);
                    marker.pose.orientation.x = 0;
                    marker.pose.orientation.y = 0;
                    marker.pose.orientation.z = sin(obj.heading / 2);

                    // std::cout << "～～～～～～～～～ok  " << std::endl;
                    markers.markers.push_back(marker);

                    visualization_msgs::msg::Marker marker_text;
                    // marker_text.lifetime = std::chrono::duration(0.1); // 存在0.1s
                    // 添加文字数组
                    marker_text.header.frame_id = "base_link";
                    marker_text.header.stamp = rclcpp::Clock().now();
                    // 为这个形状设置命名空间及id 从而创建独一无二的目标 若出现新的目标，其与旧目标有相同的命名空间及id，那么新目标会覆盖旧的目标；
                    marker_text.ns = "LidarArray_shapes_Text";
                    marker_text.id = i;
                    marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
                    marker_text.action = visualization_msgs::msg::Marker::ADD;
                    marker_text.pose.position.x = obj.x;
                    marker_text.pose.position.y = obj.y;
                    marker_text.pose.position.z = obj.z;
                    marker_text.scale.x = 1;
                    marker_text.scale.y = 1;
                    marker_text.scale.z = 1;
                    marker_text.color.r = 255;
                    marker_text.color.g = 255;
                    marker_text.color.b = 0;
                    marker_text.color.a = 1.0;
                    marker_text.lifetime.sec = 0;
                    marker_text.lifetime.nanosec = 1 * 10e7;
                    std::ostringstream str;
                    // std::cout << "obj id:" << obj.id << std::endl;
                    str << "vx,vy,id = " << int(obj.vx * 3.6) << "," << int(obj.vy * 3.6) << "," << static_cast<int>(obj.id) << std::endl;
                    // str << "id = " << msg.objs[i].idx << std::endl;
                    marker_text.text = str.str();
                    markers2.markers.emplace_back(marker_text);
                    i++;

                    visualization_msgs::msg::Marker marker_points;
                    // 设置frame_id为你的参考坐标系
                    marker_points.header.frame_id = "base_link";
                    marker_points.header.stamp = rclcpp::Clock().now();
                    marker_points.id = i;
                    // 使用当前时间戳

                    // 设置Marker类型为POINTS
                    marker_points.type = visualization_msgs::msg::Marker::POINTS;
                    // 设置颜色
                    marker_points.color.r = 0.0f;
                    marker_points.color.g = 1.0f;
                    marker_points.color.b = 0.0f;
                    marker_points.color.a = 1.0;

                    // 设置点的大小
                    marker_points.scale.x = 0.5;
                    marker_points.scale.y = 0.5;
                    marker_points.scale.z = 0.5;
                    marker_points.lifetime.sec = 0;
                    marker_points.lifetime.nanosec = 1 * 10e7;
                    if (std::fabs(obj.vx) < 3)
                        continue;
                    for (auto center_point : obj.corner_points)
                    {
                        // std::cout << "obj.id:" << obj.id << " center_point.x = " << center_point.x << " center_point.y = " << center_point.y << std::endl;
                        geometry_msgs::msg::Point p;
                        p.x = center_point.x;
                        p.y = center_point.y;
                        p.z = 0;
                        marker_points.points.push_back(p);
                    }
                    markers3.markers.emplace_back(marker_points);
                }
                RCLCPP_INFO(this->get_logger(), "lidarobjsvis publish ");
                lidar_objs_pub->publish(markers);
                lidar_objs_text_pub->publish(markers2);
                lidar_objs_track_pub->publish(markers3);
            }
        }
    };
}